# coding:utf-8

# 加入摄像头模块，让小车实现自动循迹行驶
# 思路为：摄像头读取图像，进行二值化，将白色的赛道凸显出来
# 选择下方的一行像素，黑色为0，白色为255
# 找到白色值的中点
# 目标中点与标准中点(160)进行比较得出偏移量
# 根据偏移量，采用PID控制器来控制小车左右轮的转速

import RPi.GPIO as gpio
import threading
import time
import cv2
import numpy as np

InitialSpeed = 100
kp = 130
ki = 0
kd = 2
delaytime = 0

kp *= 0.01;ki *= 0.01;kd *= 0.01

# 定义引脚
pin1 = 12
pin2 = 16
pin3 = 18
pin4 = 22
pin5 = 40


# 设置GPIO口为BOARD编号规范
gpio.setmode(gpio.BOARD)

# 设置GPIO口为输出
gpio.setup(pin1, gpio.OUT);gpio.setup(pin2, gpio.OUT);gpio.setup(pin3, gpio.OUT);gpio.setup(pin4, gpio.OUT)
gpio.setup(pin5, gpio.OUT)


# 设置PWM波,频率为600Hz
pwm1 = gpio.PWM(pin1, 600);pwm2 = gpio.PWM(pin2, 600);pwm3 = gpio.PWM(pin3, 600);pwm4 = gpio.PWM(pin4, 600)
pwm5 = gpio.PWM(pin5, 50)

# pwm波控制初始化
# 左边电机
pwm1.start(0);pwm2.start(0)
# 右边电机
pwm3.start(0);pwm4.start(0)
# 舵机
pwm5.start(0)

#循迹摄像头
cap = cv2.VideoCapture(0)
cap.set(3, 320);cap.set(4, 240)

#颜色识别摄像头
cap2 = cv2.VideoCapture(2)
cap2.set(3, 320);cap2.set(4, 240)


class PID:
    def __init__(self, InitialSpeed, kp, ki, kd):
        self.maxrange = 100#最大速度
        self.JumpSpeed = 5#跳过最低速度

        self.kp = kp;self.ki = ki;self.kd = kd

        self.pwm = 0;self.err = 0;self.last_err = 0
        self.InitialSpeed = InitialSpeed
        self.pwm_range = self.maxrange + self.InitialSpeed

        self.maxJumpSpeed = self.InitialSpeed + self.JumpSpeed
        self.minJumpSpeed = self.InitialSpeed - self.JumpSpeed

    def GetPID(self, now, target):
        self.err = target - now
        self.pwm = (
                self.pwm
                + self.kp * (self.err - self.last_err)
                + self.ki * self.err
                + self.kd * (self.err - self.last_err)
        )
        if self.pwm >= self.pwm_range:
            self.pwm = self.pwm_range
        elif self.pwm <= -self.pwm_range:
            self.pwm = -self.pwm_range
        elif self.minJumpSpeed < self.pwm < self.maxJumpSpeed:
            self.pwm = self.maxJumpSpeed
        elif -(self.maxJumpSpeed) < self.pwm < -(self.minJumpSpeed):
            self.pwm = -self.maxJumpSpeed
        self.last_err = self.err
        return self.pwm

    def getPWM(self, now):
        if isinstance(now, int):
            # if now == 0:
            #self.pwm = 0
            ret = self.GetPID(now, 0)
            # print(ret)
            Left = self.InitialSpeed - ret
            Right = self.InitialSpeed + ret
            if Left > self.maxrange:
                Left = self.maxrange
            elif -self.JumpSpeed < Left < self.JumpSpeed:
                Left = self.JumpSpeed
            if Right > self.maxrange:
                Right = self.maxrange
            elif -self.JumpSpeed < Right < self.JumpSpeed:
                Right = self.JumpSpeed
            return [Left, Right]
        else:
            return [100, 100]


def DutyChange(duty):  # 换算成电机端口输出信号
    # (100,0)==-100
    # (0,100)==100
    dutylist = [0, 0]
    if duty == 0:  # 停
        dutylist = [0, 0]
    elif duty < 0:  # 反转
        dutylist = [0, -round(duty, 2)]#round(duty, 2)小数保留2位
    elif duty > 0:  # 正转
        dutylist = [round(duty, 2), 0]
    return dutylist


def MotorBack():  # 车子后退
    print('Back')
    # 左电机
    pwm1.ChangeDutyCycle(0);pwm2.ChangeDutyCycle(50)
    # 右电机
    pwm3.ChangeDutyCycle(0);pwm4.ChangeDutyCycle(50)
    time.sleep(0.1)


def MotorOutput(PidData, stopflag):
    time.sleep(delaytime)
    if stopflag != 's':  # 循迹行走
        LeftMotorsDutylist = DutyChange(PidData[0]);RightMotorsDutylist = DutyChange(PidData[1])
        #print(str(LeftMotorsDutylist).center(10)+str(RightMotorsDutylist).center(10))
        # 左电机
        pwm1.ChangeDutyCycle(int(LeftMotorsDutylist[0]));pwm2.ChangeDutyCycle(int(LeftMotorsDutylist[1]))
        # 右电机
        pwm3.ChangeDutyCycle(int(RightMotorsDutylist[0]));pwm4.ChangeDutyCycle(int(RightMotorsDutylist[1]))
    else:
        pass


def MotorStop(t):  # 停车
    pwm1.ChangeDutyCycle(100);pwm2.ChangeDutyCycle(100)
    pwm3.ChangeDutyCycle(100);pwm4.ChangeDutyCycle(100)
    time.sleep(t)


def GetCenterPix():  # 获取黑线中心位置偏移量
    ret, frame = cap.read()
    # 转化为灰度图
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    # 大津法二值化
    retval, dst = cv2.threshold(gray, 0, 255, cv2.THRESH_OTSU)
    #retval, dst = cv2.threshold(gray, 125, 255, cv2.cv2.THRESH_BINARY)
    # cv2.imshow("im",dst)
    # 膨胀，白区域变大
    dst = cv2.dilate(dst, None, iterations=2)
    # # 腐蚀，白区域变小
    # dst = cv2.erode(dst, None, iterations=6)
    # 单看第160行的像素值s
    color = dst[120]
    # 找到黑色的像素点个数
    white_count = np.sum(color == 0)
    # if white_count >= 100:
    # 找到黑色的像素点索引
    try:
        white_index = np.where(color == 0)
        # 防止white_count=0的报错
        if white_count == 0:
            white_count = 1
        # 找到黑色像素的中心点位置
        center_now = (white_index[0][white_count - 1] + white_index[0][0]) / 2
    # 计算出center_now与标准中心点的偏移量
    except:
        return False,False
    direction = center_now - 160
    direction = 100 * direction / 130
    # print('direction:' + str(direction))
    #return direction
    return (direction, white_count)


def GetColor(lower, upper, lower0, upper0, area, color, hsvimg):
    mask = cv2.inRange(hsvimg, lower, upper)
    mask0 = cv2.inRange(hsvimg, lower0, upper0)
    mask = cv2.add(mask, mask0)
    # 图像学膨胀腐蚀
    mask = cv2.erode(mask, None, iterations=2)
    mask = cv2.GaussianBlur(mask, (3, 3), 0)
    # 寻找轮廓
    cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
    if len(cnts) > 0:
        cnt = max(cnts, key=cv2.contourArea)
        a = cv2.contourArea(cnt)
        if a > area:
            return (color)
        else:
            return ('none')
    else:
        return ('none')


# PID 初始数据

def findColor():
    global motorstopflag
    blue_lower = np.array([86, 126, 123]);blue_upper = np.array([155, 255, 255])  # 蓝色范围，最低值;最高值
    blue_lower0 = blue_lower;blue_upper0 = blue_upper
    red_lower = np.array([0, 100, 0]);red_upper = np.array([20, 255, 255])  # 红色范围，最低值;最高值
    red_lower0 = np.array([160, 100, 0]);red_upper0 = np.array([179, 255, 255])
    green_lower = np.array([35, 0, 0]);green_upper = np.array([85, 255, 255])  # 绿色范围，最低值;最高值
    green_lower0 = green_lower;green_upper0 = green_upper
    stopflag = 'f'
    colorflg = 'none'
    while 1:
        ret, frame = cap2.read()
        frame = cv2.GaussianBlur(frame, (5, 5), 0)
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
        if stopflag == 'f':  # 第一次识别到
            time2 = time.time()
            print(time2)
            if GetColor(red_lower, red_upper, red_lower0, red_upper0, 30000, 'r', hsv) == 'r':
                stopflag = 't';colorflg = 'r'
                print('getr')
            elif GetColor(blue_lower, blue_upper, blue_lower0, blue_upper0, 30000, 'b', hsv) == 'b':
                stopflag = 't';colorflg = 'b'
                print('getb')
            elif GetColor(green_lower, green_upper, green_lower0, green_upper0, 30000, 'g', hsv) == 'g':
                stopflag = 't';colorflg = 'g'
                print('getg')
            else:
                pass
        elif stopflag == 't' and time.time() - time2 > 0.3:  # 第二次识别到
            if GetColor(red_lower, red_upper, red_lower0, red_upper0, 30000, 'r', hsv) == 'r' and colorflg == 'r':
                print('getr')
                motorstopflag = 's'
                MotorStop(1.3)
                motorstopflag = 'g'
                stopflag = 'g'
            elif GetColor(blue_lower, blue_upper, blue_lower0, blue_upper0, 30000, 'b', hsv) == 'b' and colorflg == 'b':
                print('getb')
                motorstopflag = 's'
                MotorStop(1.3)
                motorstopflag = 'g'
                stopflag = 'g'
            elif GetColor(green_lower, green_upper, green_lower0, green_upper0, 30000, 'g',hsv) == 'g' and colorflg == 'g':
                print('getg')
                motorstopflag = 's'
                MotorStop(1.3)
                motorstopflag = 'g'
                stopflag = 'g'
            else:
                pass
        else:
            pass

def UphillOrDownhill(speed):#上坡或下坡
    pwm1.ChangeDutyCycle(speed);pwm2.ChangeDutyCycle(0);pwm3.ChangeDutyCycle(speed);pwm4.ChangeDutyCycle(0)

class myThreadTracking(threading.Thread):  # 第一个线程
    def run(self):
        print('Thread Tracking already started')
        times = 0;time2 = 0
        while 1:
            direction,white_count = GetCenterPix()
            print(white_count)
            if not direction:#没有黑线则后退
                #MotorBack()
                continue
            PidData = pid.getPWM(int(direction))#计算并获取PWM值

            if 250 <= white_count:#如果符合上台条件
                if times == 0:
                    print('get 0')
                    pwm1.ChangeDutyCycle(100);pwm2.ChangeDutyCycle(0);
                    pwm3.ChangeDutyCycle(100);pwm4.ChangeDutyCycle(0)
                    time.sleep(2)
                    UphillOrDownhill(20)
                    time.sleep(1)
                    times = times + 1
                    time2 = time.time()
                elif times == 1 and time.time() - time2 > 5:
                    print('get 1')
                    pwm1.ChangeDutyCycle(100);pwm2.ChangeDutyCycle(0);
                    pwm3.ChangeDutyCycle(80);pwm4.ChangeDutyCycle(0)
 
                    #UphillOrDownhill(100)
                    time.sleep(2)
                    UphillOrDownhill(20)
                    times = times + 1
                    time.sleep(0)
                #如果下台后偏得厉害就把pwm、err，last_err清零
                    pid.pwm = 0;pid.err = 0;pid.last_err = 0
            else:#否则正常循迹
                MotorOutput(PidData, motorstopflag)#循迹走
                KillBalloon()#判断符合则扎气球




class myThreadColor(threading.Thread):  # 第二个线程
    def run(self):
        time1 = time.time()
        print('Thread findColor already started')
        while time.time() - time1 < 25:
            continue
        findColor()  # 10秒后开始识别


def KillBalloon():  # 扎气球
    if motorstopflag == 's':
        pwm5.ChangeDutyCycle(12)#复位
        time.sleep(0.2)
        pwm5.ChangeDutyCycle(2)#气球位
        time.sleep(0.7)
        pwm5.ChangeDutyCycle(12)#复位
        time.sleep(0.4)


if __name__ == "__main__":
    pid = PID(InitialSpeed, kp, ki, kd)
    motorstopflag = 'g'
    t1 = myThreadTracking()
    t2 = myThreadColor()
    t1.setDaemon(True);t2.setDaemon(True)

    t1.start();t2.start()

    t1.join();t2.join()

